/*
 * SsrrPlanning.cpp
 *
 *  Created on: Oct 22, 2013
 *      Author: incubed
 */

#include <boost/math/constants/constants.hpp>
#include "ssrr_ompl_planner/SsrrPlanning.h"


namespace ssrr {

SsrrPlanning::SsrrPlanning() :
    		/*: ompl::app::AppBase<ompl::app::CONTROL>(constructControlSpace(), ompl::app::Motion_2D),*/
    			ompl::control::SimpleSetup(constructControlSpace()),
    			timeStep_(1e-2),
    			radius_(0.5)
//      controlSpace_(constructControlSpace())
{
	//si_.reset(new ompl::control::SpaceInformation(controlSpace_->getStateSpace(), controlSpace_));
	name_ = "ssrr_2013";
	odeSolver_.reset(new::ompl::control::ODEBasicSolver<>(si_, boost::bind(&SsrrPlanning::ode, this, _1, _2, _3)));
	setDefaultControlBounds();

	si_->setStatePropagator(odeSolver_->getStatePropagator(boost::bind(&SsrrPlanning::postPropagate, this, _1, _2)));
}


ompl::base::ScopedState<> SsrrPlanning::getDefaultStartState(void) const
{
	ompl::base::ScopedState<ompl::base::SE2StateSpace> sSE2(si_->getStateSpace());
	/*aiVector3D s = getRobotCenter(0);

    sSE2->setX(s.x);
    sSE2->setY(s.y);*/
	sSE2->setX(-5.);
	sSE2->setY(-5.);
	sSE2->setYaw(0.);
	return sSE2;
}

void SsrrPlanning::setDefaultControlBounds(void)
{
	ompl::base::RealVectorBounds cbounds(2);
	cbounds.low[0] = -0.1;
	cbounds.high[0] = 0.1;
	cbounds.low[1] = -boost::math::constants::pi<double>() * 30. / 180.;
	cbounds.high[1] = boost::math::constants::pi<double>() * 30. / 180.;
	si_->getControlSpace()->as<ompl::control::RealVectorControlSpace>()->setBounds(cbounds);
}

void SsrrPlanning::ode(const ompl::control::ODESolver::StateType& q, const ompl::control::Control *ctrl, ompl::control::ODESolver::StateType& qdot)
{
	const double *u = ctrl->as<ompl::control::RealVectorControlSpace::ControlType>()->values;

	// zero out qdot
	qdot.resize (q.size (), 0);

	qdot[0] = u[0] * cos(q[2]);
	qdot[1] = u[0] * sin(q[2]);
	qdot[2] = u[1];
}

void SsrrPlanning::postPropagate(const ompl::control::Control* control, ompl::base::State* state)
{
	// Normalize orientation value between 0 and 2*pi
    const ompl::base::SO2StateSpace* SO2 = si_->getStateSpace()->as<ompl::base::SE2StateSpace>()->as<ompl::base::SO2StateSpace>(1);
	ompl::base::SO2StateSpace::StateType* so2 = state->as<ompl::base::SE2StateSpace::StateType>()->as<ompl::base::SO2StateSpace::StateType>(1);
	SO2->enforceBounds(so2);
}


}

